#include "graph.h"

Graph::Graph()
{
    this->first_ = 0;
}

Graph::~Graph()
{
    if (this->first_)
        delete this->first_;
    this->first_ = 0;
    if (this->vertexId_)
        delete[] this->vertexId_;
    this->vertexId_ = 0;
}

void Graph::addVertex(int id, double lat, double lon)
{
    if (!this->first_)
        this->first_ = new Vertex(id, 0, 0, lat, lon);
    else
        this->first_->insertNext(id, lat, lon);
}

bool Graph::addEdge(int from, int to)
{
    Vertex* fromVertex = find(from);
    Vertex* toVertex = find(to);
    if (fromVertex && toVertex)
    {
        int weight = haversineFormula(fromVertex->lat, fromVertex->lon, toVertex->lat, toVertex->lon);
        fromVertex->connectTo(toVertex, weight);
        return true;
    }
    return false;
}

Vertex* Graph::find(int id)
{
    if (this->first_->getId() == id)
        return this->first_;
    else
        return findVertex(this->first_, id);
}

Vertex* Graph::findVertex(Vertex* first, int id)
{
    Vertex* current = first->getNext();
    if (current)
        if (current->getId() == id)
            return current;
        else
            return findVertex(current, id);
    else
        return 0;
}

void Graph::print()const
{
    if (this->first_);
    this->first_->printGraph();
}

int Graph::size()const
{
    if (this->first_)
        return this->first_->getSize();
    return 0;
}

std::vector<GNode>* Graph::getAdjacencyMatrix()
{
    std::vector<GNode>* adjMatrix = new std::vector<GNode>[size()];
    this->vertexId_ = new int[size()];
    Vertex* current = this->first_;
    while (current)
    {
        adjMatrix[current->position] = current->getListOfEdges();
        this->vertexId_[current->position] = current->getId();
        current = current->getNext();
    }
    current = 0;
    return adjMatrix;
}

int* Graph::getVertexId()
{
    return this->vertexId_;
}

int Graph::haversineFormula(double lat1, double lon1, double lat2, double lon2)
{
    double dx, dy, dz;
    lon1 -= lon2;
    lon1 *= TO_RAD, lat1 *= TO_RAD, lat2 *= TO_RAD;
    dz = sin(lat1) - sin(lat2);
    dx = cos(lon1) * cos(lat1) - cos(lat2);
    dy = sin(lon1) * cos(lat1);
    return asin(sqrt(dx * dx + dy * dy + dz * dz) / 2) * 2 * RAD;
}

